#!/ust/bin/env python3
# encoding: utf-8
"""
last edited at 2019-8-17
by Stone at BlueNet

控制器以及通信模块，接受recognizer的坐标，根据配置文件中描述的图像信息，按照给定的策略生成运动指令
并发送给下位机。
"""
from copy import copy, deepcopy
import serial

import config as cfg
import hmi
from robot_state import StateManager


EOL = copy(cfg.EOL)
class Communication(serial.Serial):
    """
    1. 为保证通信可靠，将处理串口的代码单独封装，以便于处理一些异常。
    emmmm... 但是目前不知道可能出现哪些异常，有了也不知道该怎么处理，所以就先不处理了orz。
    2. 封帧等通信协议在该类中完成，该类负责解决可靠传输负载的问题。

    由于主要还是直接使用串口的read(), write(), readline(), writeline()，所以采用了继承的方案。
    TODO(Stone): 由于设置了读超时，可能会触发serial.SerialTimeoutException，需要在结合hmi给出提示。
    TODO(Stone): 起草通信协议。
    """
    def __init__(self, port, baudrate=cfg.BAUDRATE, timeout=cfg.SERIAL_TIMEOUT, *args, **kwargs):
        super().__init__(port=port, baudrate=baudrate, timeout=timeout, *args, **kwargs)

    def heart_beat(self):
        """
        发送一个心跳包，下位机接收到后须在一定时间(timeout)内回复，否则认为连接已断开，
        检查串口状态并重新连接。
        """
        pass


class Effector:
    """
    该对象是对下位机指令的封装。该类提供诸如转向、直行等method，在这些method中执行串口
    发送指令等操作。其他模块操作这些method，就像直接操作小车相关功能一样，而不用关心指令
    具体是什么，从而使代码结构相对清晰。

    指令的格式也由该类负责。
    """
    def __init__(self, communication):
        self._comm = communication

    def _make_command(self, cmd, param=0):
        return '{}{}'.format(cmd, param)

    # 参数的正负决定旋转方向，正左负右。
    # 或者使用 rotate_left(), rotate_right()，参数为非负数角速度。
    def rotate(self, angle_velocity):
        command = self._make_command(cfg.ROTATE, int(angle_velocity))
        self._comm.write(command)

    # 参数的正负决定移动方向，正前负后。
    # 或者使用forward(), backward(), 参数为非负速度。
    def mv_fb(self, velocity):
        """MoVe Forward or Backward"""
        command = self._make_command(cfg.MV_FB, int(velocity))
        self._comm.write(command)

    # 摄像头俯仰角
    def look_up(self): pass
    def look_down(self): pass
    # def camera_pitch(self): pass

    # 机械爪动作
    def paw_up(self): pass
    def paw_down(self): pass
    def paw_open(self): pass
    def paw_close(self): pass

    def cast_ball(self): pass


class MotionController:
    """
    输入目标在图像中的坐标，输出相应的运动参数（速度、角速度，etc）。

    该类只负责控制小车的移动，集中了一系列控制策略。同一个任务可能有不同的控制策略，一种控制策略封装为一个method，
    顶级模块使用时可替换选用相应的method，从而采用不同的控制策略。输入统一为二维坐标，输出统一为速度、角速度。
    """
    def __init__(self):
        self.ctr_x = cfg.IMG_SIZE[0]/2  # 中轴线x坐标
        self._POS_PAW = copy(cfg.POS_PAW)

    def feedback_control(self, pos_ball):
        angular_velocity, velocity = -100, -100
        return angular_velocity, velocity


if __name__ == '__main__':

    # ===== ===== ===== ===== ===== ===== ===== ===== ===== ===== =====
    # 先运行robot_state.py，在运行本块代码，以测试是否可以共享state
    StateManager.register('get_state')
    manager = StateManager(
        address=deepcopy((cfg.MANAGER_IP, cfg.MANAGER_PORT)),
        authkey=copy(cfg.MANAGER_AUTHKEY)
    )

    manager.connect()
    state = manager.get_state()

    import time
    for i in range(50):
        print(state.get_pos_ball())
        time.sleep(1)
